/**
 * @file   MiraP3Arm.h
 * @author Josep Sabater (jsabaterm@el.ub.edu)
 * @date   November, 2016
 * @version 1.0.0
 * @ingroup QMira
 * @brief  Miradas Probe Arm
 *
 */
#ifndef MIRA_P3_ARM_H
#define MIRA_P3_ARM_H

#include <QMatrix>

#include <MiraVector2D.h>
#include "MiraIRobot2D.h"

namespace QMira{
    
    /**
    * @class Arm
    * @brief Low level details of a probe arm
    */    
    class P3Arm : public IRobot2D{
    public:
        
         /**
         * @brief Properties that define arm lengths in mm,
         */ 
        struct Lengths {
            /**
             * @brief Constructor
             */            
            Lengths( const double dL1,
                     const double dL2,
                     const double dL3,
                     const double dL4) 
                        : dL1_(dL1)
                        , dL2_(dL2)
                        , dL3_(dL3)
                        , dL4_(dL4){};
                            
            const double dL1_, dL2_, dL3_, dL4_; // values in millimeters
        };
        
        /**
         * @brief Constructor
         * @param origFrame location of the arm's frame according to the world 
         * frame. Orientation comes in degrees and position in mm.
         */         
        P3Arm(const MiraGeometryLib::Placement2D &origFrame,
              const P3Arm::Lengths &aLen);
        
        /**
         * @brief Virtual destructor
         */  
        ~P3Arm() {};
        
        /**
         * @brief Returns an unique id identifying the robot.
         */          
        IRobot2DId_t id() const override;
        
        /**
         * @brief Returns the robot status (getter)
         */          
        Status status() const override;
        
        /**
         * @brief Sets the robot status (setter)
         */               
        void status(const Status &st) override;
        
        /**
         * @brief Returns (RVO) the qs of the robot according to the World frame.
         */
        MiraUtilUBLib::DoubleCont_t getQs() const override;

        /**
         * @brief Returns (RVO) the placement of all joints
         * @param qs a container of state variables (q). Lengths in mm. and
         * angles in degrees.
         * The joint placements is according to the World frame. 
         * Position comes in mm and orientation in degrees.
         * 
         */
        Joint2D::Joint2DCont_t getJoints(const MiraUtilUBLib::DoubleCont_t &qs) const override;        
        
        /**
         * @brief Returns the link length in mm.
         */
        const MiraUtilUBLib::DoubleCont_t& getLinksLength() const override;        
        
        /**
         * @brief Translate a given point in the robot frame into 
         *a point in the World frame.
         * @param aPoint a 2D point expressed according to robot frame.
         */
        MiraGeometryLib::Vector2D robot2World(const MiraGeometryLib::Vector2D &aPoint) const override;

        /**
         * @brief Translate a given point in the World frame into 
         *a point in the robot  frame.
         * @param aPoint a 2D point expressed according to World frame.
         */
        MiraGeometryLib::Vector2D world2Robot(const MiraGeometryLib::Vector2D &wPoint) const override;

        /**
         * @brief Build a string with representative information of the arm.
         */
         std::string str() const override;
         
    private:
        const IRobot2DId_t id_;
        IRobot2D::Status status_;
        const MiraGeometryLib::Vector2D j1PosWorld_;
        const MiraGeometryLib::Vector2D j2PosWorld_;
        const P3Arm::Lengths lengths_;
        
        double thetaOffset_;
        QMatrix transRobot2World_; //!< Transformation matrix

        static IRobot2D::IRobot2DId_t idCntr_;
        
        MiraGeometryLib::Vector2D initJ2World_(const MiraGeometryLib::Placement2D &j1PosWorld,
                                               const P3Arm::Lengths &len) const;
        /* incoming value in radians, while outcome is in degrees */
        MiraGeometryLib::Placement2D whereIsJ1_(const double theta1) const;
        /* incoming value in radians, while outcome is in degrees */
        MiraGeometryLib::Placement2D whereIsJ2_(const double theta2) const;
        /* incoming value in radians, while outcome is in degrees */
        MiraGeometryLib::Placement2D whereIsJ3_(const double theta1) const;
        /* incoming value in radians, while outcome is in degrees */
        MiraGeometryLib::Placement2D whereIsJ4_(const double theta2) const;
        
    };
}

#endif /*MIRA_ARM_H*/